/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#define MAP_FILE_NAME "../../res/autmap-small.txt"

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include <string>
#include <time.h>
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "CParticleFilter.h"
#include "glutImageGUI.h"   ///< For visualisation
#include "JPEGEncoder.hpp"  ///< For saving JPEG


void writeMottiMissionFile(std::string fname, std::vector<Grid::TCoord2D> &coords){
	srand(time(NULL));
	FILE *f = fopen(fname.c_str(),"wt");
	
	while(coords.size()>0){
		int rind = rand()%coords.size();
		
		fprintf(f, "mission 0 Marsu\n");
		fprintf(f,"deliver %f %f %d\n",coords[rind].x,coords[rind].y, rind);
		
		coords.erase(coords.begin()+rind);
	}

}

float topi(float da){
	if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
	else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
	return da;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
		rgb[3*Nx*j+3*i]=r;
		rgb[3*Nx*j+3*i+1]=g;
		rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
		float *data = gr->getDataPtr();
		int Nx,Ny;
		gr->getDim(Nx,Ny);
		float v;
	
		for(int j=0;j<Ny;j++){
				for(int i=0;i<Nx;i++){
						v = data[Nx*j+i];
						if(v>0.6) v = 0; else v = 0.99; 
				
						setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
				}
		}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpixBold(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
									float dy = sin(angle);
									float dx = cos(angle);
									float rr=0;
									int i=0;
									uint x,y;
									while(rr<length){
											x = pix + i*dx;
											y = piy - i*dy;
											setRGBpix(RGB,x,y, Nx, r,g,b);
											i++;
											rr+=resolution;
									}
							}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float angle = M_PI/4;
		for(int i=0;i<3;i++){
				drawLineThin(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
				angle+=M_PI/2;
		}
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

float randomi(){
	return (2.0 * (float)(rand()%10000) - 1.0)/10000.0;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

void printUsage(){
		fprintf(stderr,"\n-----------------------------------------------------------------\n");
		fprintf(stderr,"Plots an occupancy grid from a log file\n");
		fprintf(stderr,"Usage: ./plot_occu_map [in_file]\n");
		fprintf(stderr,"\nThe output is saved to 'out_result.jpg'\n");
		fprintf(stderr,"-----------------------------------------------------------------\n\n");
}



int main(int argc, char *argv[]){
		const char *measurement_file_name; 
		
		if (argc != 2){
				printUsage();
				exit(1);
		}
		
		measurement_file_name = argv[1];
		
		TOccupancyGrid O;
		O.load(measurement_file_name);		
		
		int Nx,Ny;	
		unsigned char *rgb;
		O.getDim(Nx,Ny);
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		
		float *disttf = (float *) malloc(sizeof(float)*Nx*Ny);
		
		float maxdist = O.distanceTransform(disttf, 0.2);
		
		float *dptr=O.getDataPtr();
		
		float paramDistanceth = 0.45;
		
		for(int i=0;i<O.size();i++){
			if(dptr[i]<0.2){
				dptr[i]=0;
			}else dptr[i]=1.0;
		}
		
		O.fillRGB(rgb,1.0/255.0);
		saveJPEG("bin_output.jpg", rgb,Nx,Ny);	
		
		///Make thresholding
		for(int i=0;i<O.size();i++){
			if(disttf[i]<paramDistanceth){
				dptr[i]=1.0;
			}
		}
		
		
		std::vector<Grid::TCoord2D> targets;
		for(int i=300;i<Nx;i+=20){
			for(int j=235;j<360;j+=20){
				if(dptr[i+Nx*j]<0.1){
					Grid::TCoord2D coord = O.toCoord(Grid::TPixel(i,j));
					targets.push_back(coord);
				}
				
			}
		}
		fprintf(stderr,"HAS %d Targets\n",targets.size());
		
		
		
		//O.fillRGB(rgb,1.0/255.0);
		
		saveJPEG("out_result.jpg", rgb,Nx,Ny);	
		
		
		
		
		
		if(Nx > TEXTURE_WIDTH || Ny>TEXTURE_HEIGHT){
				fprintf(stderr,"The map is WAY too big to display\n");
				fprintf(stderr,"Showing a submap instead\n");
				
				float resolution = O.getResolution();
				TOccupancyGrid lmap(TEXTURE_WIDTH,TEXTURE_HEIGHT,resolution, Grid::TCoord2D(0,0));
			  float *ptr = lmap.getDataPtr();
				int nx,ny;
				lmap.getDim(nx,ny);
				fprintf(stderr,"Local map size = (%dx%d)",nx,ny);
				O.getSubmap(ptr, Grid::TPixel(Nx/2,Ny/2), TEXTURE_WIDTH, TEXTURE_HEIGHT);
				
				free(rgb);
				Nx = nx;Ny=ny;
				rgb = (unsigned char *) malloc(3*Nx*Ny);
				lmap.fillRGB(rgb,1.0/255.0);
		}
		
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		for(int i=0;i<targets.size();i++){
			Grid::TPixel pix = O.toPixel( targets[i] );
			setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0);
			
		}
		writeMottiMissionFile("Motti_mission.msmf",targets);
		fprintf(stderr,"\n ######   CTRL-C Ends the show   ######\n");
		while(1){
				
			
			
				displayImage(0, rgb,Ny,Nx);
				usleep(1000*100);
		}

		free(rgb);
		
		return 0;
}		